from geometry_msgs.msg import PoseStamped #PoseStamped：ROS 2 的标准消息类型，用于表示带时间戳和坐标系的三维位姿（位置 + 方向）。
from nav2_simple_commander.robot_navigator import BasicNavigator #BasicNavigator：Nav2 提供的简化接口，封装了导航任务（如设置初始位姿、发送目标点）。
import rclpy

def main():
    rclpy.init()
    nav = BasicNavigator()    #创建 BasicNavigator 实例，它会自动启动 Nav2 的关键节点（如 controller_server、planner_server）。
    #设定位置信息
    init_pose = PoseStamped()                
    init_pose.header.frame_id = "map"
    init_pose.header.stamp = nav.get_clock().now().to_msg()
    init_pose.pose.position.x =0.0
    init_pose.pose.position.y =0.0
    init_pose.pose.orientation.w =1.0

    nav.setInitialPose(init_pose)  #将初始位姿发布到 /initialpose 话题，AMCL 会据此初始化机器人的定位。 AMCL是机器人定位算法，主要用于已知地图环境下的2D移动机器人定位
    nav.waitUntilNav2Active()      #阻塞直到 Nav2 的所有服务启动完成

    rclpy.spin(nav)
    rclpy.shutdown()
